#include <buola/image/algorithm/statistics.h>
#include <buola/image/algorithm/filter.h>
#include <buola/image/algorithm/structure.h>

namespace buola { namespace img {

float image_max(const img::CView_gray32f &pImage)
{
  const float *imd = pImage.RawData();
  int h = pImage.Height();
  int w = pImage.Width();;
  float tmp = 0;

#ifdef BUOLA_USE_INTRINSICS
    __m128 max4 = _mm_setzero_ps();
    float tmp4[4];
    for (int i=0; i<w*h; i+=16) {
      max4 = _mm_max_ps(max4, _mm_loadu_ps(&imd[i+0]));
      max4 = _mm_max_ps(max4, _mm_loadu_ps(&imd[i+4]));
      max4 = _mm_max_ps(max4, _mm_loadu_ps(&imd[i+8]));
      max4 = _mm_max_ps(max4, _mm_loadu_ps(&imd[i+12]));
    }
    _mm_storeu_ps(tmp4, max4);
    float lom = (tmp4[0]>tmp4[1] ? tmp4[0] : tmp4[1]);
    float him = (tmp4[2]>tmp4[3] ? tmp4[2] : tmp4[3]);
    tmp = (lom>him ? lom : him);
#else
    for (int i=0; i<w*h; i++)
      if (imd[i]>tmp)
        tmp = imd[i];
#endif
  return tmp;
}


float image_min(const img::CView_gray32f &pImage)
{
  const float *imd = pImage.RawData();
  int h = pImage.Height();
  int w = pImage.Width();

  float tmp = imd[0];
#ifdef BUOLA_USE_INTRINSICS
  __m128 min4 = _mm_set1_ps(tmp);
  float tmp4[4];
  for (int i=0; i<w*h; i+=16) {
    min4 = _mm_min_ps(min4, _mm_load_ps(&imd[i+0]));
    min4 = _mm_min_ps(min4, _mm_load_ps(&imd[i+4]));
    min4 = _mm_min_ps(min4, _mm_load_ps(&imd[i+8]));
    min4 = _mm_min_ps(min4, _mm_load_ps(&imd[i+12]));
  }
  _mm_storeu_ps(tmp4, min4);
  float lom = (tmp4[0]<tmp4[1] ? tmp4[0] : tmp4[1]);
  float him = (tmp4[2]<tmp4[3] ? tmp4[2] : tmp4[3]);
  tmp = (lom<him ? lom : him);
#else
  for (int i=0; i<w*h; i++)
    if(imd[i]<tmp)
      tmp = imd[i];
#endif
  return tmp;
}

void image_minmax(const img::CView_gray32f &pImage,float &pMin,float &pMax)
{
  const float *imd = pImage.RawData();
  int h = pImage.Height();
  int w = pImage.Width();

#ifdef BUOLA_USE_INTRINSICS
    float tmp = imd[0];
    __m128 min4=_mm_set1_ps(tmp);
    __m128 max4=min4;
    float tmp4[4];
    for (int i=0; i<w*h; i+=4)
    {
        __m128 x=_mm_load_ps(&imd[i+0]);
        min4=_mm_min_ps(min4,x);
        max4=_mm_max_ps(max4,x);
    }
    _mm_storeu_ps(tmp4, min4);
    float lom = (tmp4[0]<tmp4[1] ? tmp4[0] : tmp4[1]);
    float him = (tmp4[2]<tmp4[3] ? tmp4[2] : tmp4[3]);
    pMin=(lom<him ? lom : him);
    _mm_storeu_ps(tmp4, max4);
    lom = (tmp4[0]>tmp4[1] ? tmp4[0] : tmp4[1]);
    him = (tmp4[2]>tmp4[3] ? tmp4[2] : tmp4[3]);
    pMax=(lom>him ? lom : him);
#else
    float lMin=imd[0];
    float lMax=imd[0];
    for (int i=0; i<w*h; i+=2)
    {
        if(imd[i]<imd[i+1])
        {
            if(imd[i]<lMin)
                lMin=imd[i];
            if(imd[i+1]>lMax)
                lMax=imd[i+1];
        }
        else
        {
            if(imd[i]>lMax)
                lMax=imd[i];
            if(imd[i+1]<lMin)
                lMin=imd[i+1];
        }
    }
    pMin=lMin;
    pMax=lMax;
#endif
}

float local_max_mean(const img::CView_gray32f &pImage,float pAbsMax)
{
    const float *lData = pImage.RawData();
    int h = pImage.Height();
    int w = pImage.Width();
    float lSum=0.0f;
    int lCount=0;

    for (int y=1;y<h-1;y+=2)
    {
        const float *lPR=lData+(y-1)*w;
        const float *lR1=lData+y*w;
        const float *lR2=lData+(y+1)*w;
        const float *lNR=lData+(y+2)*w;

        for (int x=1;x<w-1;x+=2)
        {
            float l11=lR1[x];
            float l12=lR1[x+1];
            float l21=lR2[x];
            float l22=lR2[x+1];

            if(l11>l12) //l11>l12
            {
                if(l11>l21) //l11>(l12,l21)
                {
                    if(l11>l22) //l11>(l12,l21,l22)
                    {
                        //l11
                        if(l11>lR1[x-1]&&l11>lPR[x-1]&&l11>lR2[x-1]&&l11>lPR[x]&&l11>lPR[x+1])
                        {
                            lSum+=l11;
                            lCount++;
                        }
                    }
                    else if(l22>l11) //l22>l11>(l12,l21)
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l11=l22)>(l12,l21)
                }
                else if(l21>l11) //l21>l11>l12
                {
                    if(l21>l22) //l21>(l22,l11>l12)
                    {
                        //l21
                        if(l21>lR1[x-1]&&l21>lR2[x-1]&&l21>lNR[x-1]&&l21>lNR[x]&&l21>lNR[x+1])
                        {
                            lSum+=l21;
                            lCount++;
                        }
                    }
                    else if(l22>l21) //l22>l21>l11>l12
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l22=l21)>l11>l12
                }
                else //(l11=l21)>l12
                {
                    if(l22>l11) //l22>(l11=l21)>l12
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l11=l21)>=(l12,l22)
                }
            }
            else if(l12>l11) //l12>l11
            {
                if(l12>l21) //l12>(l21,l11)
                {
                    if(l12>l22) //l12>(l21,l11,l22)
                    {
                        //l12
                        if(l12>lR2[x+2]&&l12>lR1[x+2]&&l12>lPR[x+2]&&l12>lPR[x+1]&&l12>lPR[x])
                        {
                            lSum+=l12;
                            lCount++;
                        }
                    }
                    else if(l22>l12) //l22>l12>(l21,l11)
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l22=l12)>(l21,l11)
                }
                else if(l21>l12) //l21>l12>l11
                {
                    if(l21>l22) //l21>(l22,l12>l11)
                    {
                        //l21
                        if(l21>lR1[x-1]&&l21>lR2[x-1]&&l21>lNR[x-1]&&l21>lNR[x]&&l21>lNR[x+1])
                        {
                            lSum+=l21;
                            lCount++;
                        }
                    }
                    else if(l22>l21) //l22>l21>l12>l11
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l22=l21)>l12>l11
                }
                else //(l21=l12)>l11
                {
                    if(l22>l12) //l22>(l21=l12)>l11
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l21=l12)>=(l11,l22)
                }
            }
            else //l12=l11
            {
                if(l21>l22) //l21>l22,l12=l11
                {
                    if(l21>l11) //l21>(l22,l11=l12)
                    {
                        //l21
                        if(l21>lR1[x-1]&&l21>lR2[x-1]&&l21>lNR[x-1]&&l21>lNR[x]&&l21>lNR[x+1])
                        {
                            lSum+=l21;
                            lCount++;
                        }
                    }
                    //(l11=l12)>=l21>l22
                }
                else if(l22>l21) //l22>l21,l12=l11
                {
                    if(l22>l11) //l22>(l21,l11=l12)
                    {
                        //l22
                        if(l22>lR1[x+2]&&l22>lR2[x+2]&&l22>lNR[x+1]&&l22>lNR[x+2]&&l22>lNR[x])
                        {
                            lSum+=l22;
                            lCount++;
                        }
                    }
                    //(l11=l12)>=l22>l21
                }
                //l22=l21,l11=l12
            }
        }
    }

    if(lCount>1)
        return (lSum - pAbsMax)/(lCount - 1);
    return 0.0f;
}

void add_and_scale(const img::CMView_gray32f &pSrc,float pAdd,float pScale)
{
    //pSrc=pScale*(pSrc+pAdd)
    int lSize=pSrc.Width()*pSrc.Height();
    float *lI=pSrc.RawData();
    #ifdef BUOLA_USE_INTRINSICS
    __m128 lAdd=_mm_set1_ps(pAdd);
    __m128 lScale=_mm_set1_ps(pScale);
    for(int i=0;i<lSize;i+=16)
    {
        _mm_store_ps(&lI[i+0],_mm_mul_ps(lScale,_mm_add_ps(_mm_load_ps(&lI[i+0]),lAdd)));
        _mm_store_ps(&lI[i+4],_mm_mul_ps(lScale,_mm_add_ps(_mm_load_ps(&lI[i+4]),lAdd)));
        _mm_store_ps(&lI[i+8],_mm_mul_ps(lScale,_mm_add_ps(_mm_load_ps(&lI[i+8]),lAdd)));
        _mm_store_ps(&lI[i+12],_mm_mul_ps(lScale,_mm_add_ps(_mm_load_ps(&lI[i+12]),lAdd)));
    }
    #else
    for(int i=0;i<lSize;i++)
    {
        lI[i]=pScale*(lI[i]+pAdd);
    }
    #endif
}

void pow1p5(const img::CMView_gray32f &pSrc)
{
    int lSize=pSrc.Width()*pSrc.Height();
    float *lI=pSrc.RawData();
    #ifdef BUOLA_USE_INTRINSICS
    for (int i=0; i<lSize; i+=4)       // faster alternative imd[i]^1.5
    {
        __m128 v=_mm_load_ps(lI+i);
        __m128 a=_mm_rcp_ps(_mm_rsqrt_ps(v)); //this approximation is faster than the square root
        _mm_store_ps(lI+i, _mm_mul_ps(a, v));
    }
    #else
    for (int i=0; i<lSize; i++)       // faster alternative imd[i]^1.5
    {
        lI[i]*=sqrt(lI[i]);
    }
    #endif
}

void gabor_filter(const img::CView_gray32f &pSrc,
                  const img::CMView_gray32f &pO0,const img::CMView_gray32f &pO45,
                  const img::CMView_gray32f &pO90,const img::CMView_gray32f &pO135)
{
    int w=pSrc.Width(),h=pSrc.Height();

    img::CImage_gray32f lBlur(pSrc.Size());

    gaussian_blur(pSrc,lBlur);
    gaussian_blur(lBlur);
    const float *lSrc=lBlur.RawData();
    float *lO0=pO0.RawData();
    float *lO45=pO45.RawData();
    float *lO90=pO90.RawData();
    float *lO135=pO135.RawData();

    for (int y=0;y<4;y++)
    {
        int lR=y*w;

        for (int x=0;x<w;x++)
            lO0[lR+x]=lO45[lR+x]=lO90[lR+x]=lO135[lR+x]=0.0f;
    }
    for (int y=4;y<h-4;y++)
    {
        int lR=y*w;

        for (int x=0;x<4;x++)
            lO0[lR+x]=lO45[lR+x]=lO90[lR+x]=lO135[lR+x]=0.0f;

        for (int x=4;x<w-4;x++)
        {
            int lP=lR+x;

            lO0[lP]=std::abs(lSrc[lP+1]-lSrc[lP-1]);
            lO45[lP]=std::abs(lSrc[lP+w-1]-lSrc[lP-w+1]);
            lO90[lP]=std::abs(lSrc[lP+w]-lSrc[lP-w]);
            lO135[lP]=std::abs(lSrc[lP+w+1]-lSrc[lP-w-1]);
        }

        for (int x=w-4;x<w;x++)
            lO0[lR+x]=lO45[lR+x]=lO90[lR+x]=lO135[lR+x]=0.0f;
    }
    for (int y=h-4;y<h;y++)
    {
        int lR=y*w;

        for (int x=0;x<w;x++)
            lO0[lR+x]=lO45[lR+x]=lO90[lR+x]=lO135[lR+x]=0.0f;
    }
}

std::array<CPoint_d,4> min_area_rect(const std::vector<CPoint_d> &pPoints)
{
    std::vector<cv::Point2f> lPoints;
    lPoints.reserve(pPoints.size());
    for(const auto &p : pPoints)
    {
        lPoints.emplace_back(p.x,p.y);
    }
    
    cv::RotatedRect lBox=cv::minAreaRect(lPoints);
    cv::Point2f lCorners[4];
    lBox.points(lCorners);
    std::array<CPoint_d,4> lResult;
    for(int i=0;i<4;i++)
    {
        lResult[i].x=lCorners[i].x;
        lResult[i].y=lCorners[i].y;
    }
    return lResult;
}

std::array<CPoint_f,4> min_area_rect(const std::vector<CPoint_f> &pPoints)
{
    std::vector<cv::Point2f> lPoints;
    lPoints.reserve(pPoints.size());
    for(const auto &p : pPoints)
    {
        lPoints.emplace_back(p.x,p.y);
    }
    
    cv::RotatedRect lBox=cv::minAreaRect(lPoints);
    cv::Point2f lCorners[4];
    lBox.points(lCorners);
    std::array<CPoint_f,4> lResult;
    for(int i=0;i<4;i++)
    {
        lResult[i].x=lCorners[i].x;
        lResult[i].y=lCorners[i].y;
    }
    return lResult;
}

/*namespace img*/ } /*namespace buola*/ }
